#include "CalibrationImageSelectorGUI.h"
#include "MiscFunctions.h"
#include "ComputeCalibrationRect.hpp"

#include <libfreenect.h>
#include <stdio.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <stdexcept>

using namespace std;
using namespace cv;
using namespace idch;

#define MESSAGE_HEIGHT 40
#define MAIN_WINDOW "Calibration Window"

VideoWriter video;

CalibrationImageSelectorGUI::CalibrationImageSelectorGUI( MotionDetection *motion , int warmupFrames )
    : findTarget(100,FREENECT_FRAME_W,FREENECT_FRAME_H) , coverage(FREENECT_FRAME_W,FREENECT_FRAME_H,0.8)
{

    this->warmupFrames = warmupFrames;
    this->motion = motion;
    this->numFrames = 0;

    this->numCapturedImages = 0;

    mainImg   = Mat::zeros(FREENECT_FRAME_H/2+MESSAGE_HEIGHT+FREENECT_FRAME_H/4, FREENECT_FRAME_W, CV_8UC3);

    drawRGB   = Mat::zeros(FREENECT_FRAME_H, FREENECT_FRAME_W, CV_8UC3);
    drawDepth = Mat::zeros(FREENECT_FRAME_H, FREENECT_FRAME_W, CV_8UC3);

    tempRGB   = Mat::zeros(FREENECT_FRAME_H, FREENECT_FRAME_W, CV_8UC3);
    tempSmallGray   = Mat::zeros(FREENECT_FRAME_H/4, FREENECT_FRAME_W/4, CV_8UC1);

    namedWindow(MAIN_WINDOW);

    if( !video.open("out.avi",CV_FOURCC('P','I','M','1'),20,cvSize(mainImg.cols,mainImg.rows),true) )
        throw std::runtime_error("video open failed");
}

CalibrationImageSelectorGUI::~CalibrationImageSelectorGUI() {

}

bool CalibrationImageSelectorGUI::
        process( cv::Mat colorImg , cv::Mat depthImg ) {

    colorImg.copyTo(drawRGB);
    convertGray16to8(depthImg, drawDepth, 0x07FF);

    numFrames++;

    if( numFrames++ <= warmupFrames ) {
        doWarmUp();
    } else if( !coverage.isImageCovered() ){
        doActive(colorImg,depthImg);
    } else {
        doCalibrate();
    }


    cv::Mat left = mainImg.rowRange(MESSAGE_HEIGHT,MESSAGE_HEIGHT+FREENECT_FRAME_H/2).colRange(0,FREENECT_FRAME_W/2);
    cv::Mat right = mainImg.rowRange(MESSAGE_HEIGHT,MESSAGE_HEIGHT+FREENECT_FRAME_H/2).colRange(FREENECT_FRAME_W/2,FREENECT_FRAME_W);

    cv::resize(drawDepth,left,Size(),0.5,0.5);
    cv::resize(drawRGB,right,Size(),0.5,0.5);

    imshow(MAIN_WINDOW,mainImg);

    if( video.isOpened() ) {
        video << mainImg;
    }

    return false;
}

const std::vector<CalibrationLocation>&
        CalibrationImageSelectorGUI::getTargets() const
{

}

void CalibrationImageSelectorGUI::saveImages() {
    char name[1024];

    sprintf(name,"images/orig_rgb_%04d.ppm",numCapturedImages);
    imwrite(name,drawRGB);
    sprintf(name,"images/orig_depth_%04d.pgm",numCapturedImages);
    imwrite(name,drawDepth);
    sprintf(name,"images/mask_rgb_%04d.pgm",numCapturedImages);
    imwrite(name,motion->getMotionRGB());
    sprintf(name,"images/mask_depth_%04d.pgm",numCapturedImages);
    imwrite(name,motion->getMotionDepth());
}

void CalibrationImageSelectorGUI::doWarmUp() {
    char message[1024];
    sprintf(message," STAY AWAY %d",(warmupFrames-numFrames+1));

    setMessage(message);
}

bool CalibrationImageSelectorGUI::doActive( cv::Mat colorImg , cv::Mat depthImg ) {
//    printf("----------------------------------- CalibrationImageSelectorGUI::doActive ------------------\n");
    int b = FREENECT_FRAME_H/4;
    int a = FREENECT_FRAME_W/4;

    cv::Mat small0 = mainImg.rowRange(MESSAGE_HEIGHT+FREENECT_FRAME_H/2,mainImg.rows).colRange(   0,  a);
    cv::Mat small1 = mainImg.rowRange(MESSAGE_HEIGHT+FREENECT_FRAME_H/2,mainImg.rows).colRange(   a, 2*a);
    cv::Mat small2 = mainImg.rowRange(MESSAGE_HEIGHT+FREENECT_FRAME_H/2,mainImg.rows).colRange( 2*a, 3*a);
    cv::Mat small3 = mainImg.rowRange(MESSAGE_HEIGHT+FREENECT_FRAME_H/2,mainImg.rows).colRange( 3*a, 4*a);

    if( motion->addFrame(colorImg,depthImg) ) {
        bool goodTarget = false;
        char message[1024];
        double covered = 100.0*coverage.getCoverage();
        sprintf(message,"Captured Images %d   %5.3f%%",numCapturedImages,covered);
        setMessage(message);

        CalibrationLocation foundTarget;

        // draw the motion image
        cv::resize(motion->getMotionDepth(),tempSmallGray,Size(),0.25,0.25);
        cvtColor(tempSmallGray, small0, CV_GRAY2BGR);
        cv::resize(motion->getMotionRGB(),tempSmallGray,Size(),0.25,0.25);
        cvtColor(tempSmallGray, small2, CV_GRAY2BGR);

        // find the target and draw the contour image
        bool foundDepth = findAndDrawTarget(motion->getMotionDepth(),foundTarget.depth,small1,true);
        bool foundRGB = findAndDrawTarget(motion->getMotionRGB(),foundTarget.rgb,small3,false);

        if( foundRGB && foundDepth ) {
            if( coverage.addTarget(foundTarget)) {
                goodTarget = true;
                saveImages();
                numCapturedImages++;
            }
        }

//        cv::Mat right = mainImg.rowRange(MESSAGE_HEIGHT,MESSAGE_HEIGHT+FREENECT_FRAME_H/2).colRange(FREENECT_FRAME_W/2,FREENECT_FRAME_W);
//        cv::resize(tempRGB,right,Size(),0.5,0.5);

        // draw a mask over the image in areas that are not yet covered
        coverage.drawCoverage(drawDepth);

        // draw targets
        if( foundDepth )
            drawTarget(foundTarget.depth,drawDepth,goodTarget);
        if( foundRGB )
            drawTarget(foundTarget.rgb,drawRGB,goodTarget);

        if( coverage.isImageCovered() )
            return true;
    } else {
        setMessage("Making Back Ground");
    }

    return false;
}

bool CalibrationImageSelectorGUI::doCalibrate() {
    ComputeCalibrationRect calib(11.0,8.5,640,480,20,3.0);

    setMessage("Calibrating");

    printf("Calibrating\n");
    calib.process(coverage.getTagets());

    saveCalibration(calib.getCalib(),FREENECT_FRAME_W,FREENECT_FRAME_H);

    printf("Done calibrating!\n");
    exit(0);
}

bool CalibrationImageSelectorGUI::findAndDrawTarget( cv::Mat& motionImg , RectPt &target , cv::Mat &smallImg ,
                                                     bool isDepth ) {

    if( isDepth ) {
        findTarget.setConfig(3.0);
    } else {
        findTarget.setConfig(1.5);
    }

    // TEMP HACK
    cv::Mat copy;
    motionImg.copyTo(copy);

    bool found = findTarget.process(copy);

    if( found ) {
        target = findTarget.getTarget();
    }

    tempRGB.setTo(Scalar(0,0,0));
    findTarget.drawContours(tempRGB);
    findTarget.drawCorners(tempRGB);
    findTarget.drawLines(tempRGB);
    cv::resize(tempRGB,smallImg,Size(),0.25,0.25);

    return found;
}

void CalibrationImageSelectorGUI::drawTarget( const RectPt& target , cv::Mat image , bool usedTarget ) {

    Scalar color;
    if( usedTarget ) {
        color = Scalar(255,0,0);
    } else {
        color = Scalar(0,0,255);
    }

    cv::line(image,target.tl,target.tr,color,3);
    cv::line(image,target.tr,target.br,color,3);
    cv::line(image,target.br,target.bl,color,3);
    cv::line(image,target.bl,target.tl,color,3);
}

void CalibrationImageSelectorGUI::setMessage( const char *message ) {
    cv::Mat textImg = mainImg.rowRange(0,MESSAGE_HEIGHT).colRange(   0,  mainImg.cols);

    int fontFace = FONT_HERSHEY_SIMPLEX;
    double fontScale = 1;
    int thickness = 2;

    int baseline=0;
    Size textSize = getTextSize(message, fontFace,
                                fontScale, thickness, &baseline);

//    printf("text size %d %d\n",textSize.width,textSize.height);

    textImg.setTo(Scalar(0,0,0));
    putText(textImg, message, Point(10,textSize.height+10), fontFace, fontScale,
            Scalar(255,255,255), thickness, 8);
}
